#include "ramps.h"
#include "Arduino.h"

#define TOL 1e-6

/**
 * Ramps Constructor
 */
Ramps::Ramps(float hister)
{
	// Inicializa variaveis
	_initTime = -1.0;
	_hister = hister;
	_nramps = 0;
}


/**
 * AddRamp
 */
bool Ramps::AddRamp(float tempInit, float tempFinish, float duration)
{
	// Verifica numero de rampas
	if(_nramps >= MAXRAMPS)
		return false;
	
	// Adiciona rampa e atualiza numero
	_ramps[_nramps]._tempInit = tempInit;
	_ramps[_nramps]._tempFinish = tempFinish;
	_ramps[_nramps]._duration = duration;
	_nramps++;
	
	return true;
}


/**
 * RiseTemp
 */
char Ramps::RiseTemp(float time, float temperature)
{
	float setpoint = 0.0;
	// Recupera o setpoint que deveria estar
	// a variavel setpoint eh passada como referencia
	// ela sera modificada dentro da outra funcao, mas como
	// eh referencia, o valor eh mantido
	// Verifica se deu algum erro ao obter o setpoint
	if(!_getSetPoint(time, setpoint))
		return -1;

	// Verifica se a temperatura atual esta acima o abaixo da setpoint
	return (temperature <= (setpoint - _hister));
}


/**
 * _getSetPoint
 */
bool Ramps::_getSetPoint(float time, float &setpoint)
{
	// Verifica se tempo inicial foi inicializado
	if(_initTime < (0.0-TOL))
		return false;

	// Verifica se ha rampas definidas
	if(_nramps == 0)
		return false;

	// Verifica validade de tempo dado
	if(time < (0.0-TOL))
		return false;

	// Pega o intervalo de tempo entre agora e o inicio das rampas
	float interval = time - _initTime;
	if(interval < (0.0-TOL))
		return false;

	// Variavel que guarda somatoria das duracoes das rampas
	float dursum = 0.0;
	// Varre as rampas e verifica em qual rampa esta
	for(unsigned char i = 0; i<_nramps; i++)
	{
		// Calculo o fator de escala entra a temperatura inicial e final da rampa
		// setpoint = factor*(temperatura_final - temperatura_inicial) + temperatura_inicial
		// Subtrai do intervalo total a soma das duracoes das rampas que foram verificadas
		// previamente no loop
		float factor = (interval - dursum)/_ramps[i]._duration;
		// Se factor for maior que 1, signfica que o tempo corrente esta
		// em uma rampa subsequente
		if (factor>1.0)
		{
			// Atualiza o somatorio das duracoes das rampas varridas
			dursum += _ramps[i]._duration;
			// Vai para proximo passo do for
			continue;
		}
		
		// Se o tempo corrente estiver dentro da rampa i, calcula setpoint
		// Valor de setpoint eh guardado dentro do parametro da funcao
		// que eh passado como referencia, assim esse valor eh guardado
		setpoint = factor*(_ramps[i]._tempFinish - _ramps[i]._tempInit) + _ramps[i]._tempInit;
Serial.print("SetPoint: ");Serial.print(setpoint);Serial.print("\n");
		// Ja que achou o ponto nas rampas correspondente ao tempo atual, pode sair da funcao
		return true;
	}

	// Se chegou ate aqui eh pq deu merda
	return false;
}
